#!/bin/bash
# ============================================================
# Script Name:    common
# Description:    ROS2公共函数库
# Author:         haibo
# Created Date:   2025-07-24
# Version:        1.0
# ============================================================
LOG_FILE="${1:-/tmp/agv_bringup.log}"
PARAMS=$2

declare -A param_map
PROC_ENTRIES=()
LOG_LEVEL="INFO"

load_params() {
    local result
    result=$(python3 - <<EOF
import json
import rclpy
from rclpy.node import Node
from agv_srvs.srv import StaticParam

def flatten_params(data, parent_key='', sep='.'):
    items = []
    if isinstance(data, dict):
        for k, v in data.items():
            new_key = f"{parent_key}{sep}{k}" if parent_key else k
            if isinstance(v, dict):
                items.extend(flatten_params(v, new_key, sep=sep))
            else:
                items.append((new_key, v))
    return items

def main():
    rclpy.init()
    node = Node('static_param_client')
    
    try:
        client = node.create_client(StaticParam, '/StaticParam')
        
        # 等待服务可用（最多 5 秒）
        if not client.wait_for_service(timeout_sec=5.0):
            node.get_logger().error('Service /StaticParam not available')
            return "ERROR:Service not available"
        
        request = StaticParam.Request()
        request.action_name = 'read'
        
        future = client.call_async(request)
        rclpy.spin_until_future_complete(node, future)
        
        if future.result() is not None:
            response = future.result()
            try:
                # 解析 JSON 字符串
                data = json.loads(response.ack_message)
                # 展平参数结构
                params = flatten_params(data)
                # 输出为 name,value 对
                for name, value in params:
                    if isinstance(value, dict) and 'value' in value:
                        print(f"{name},{value['value']}")
                    else:
                        print(f"{name},{value}")
                return "SUCCESS"
            except json.JSONDecodeError as e:
                node.get_logger().error(f'JSON decode error: {str(e)}')
                return "ERROR:Invalid JSON format"
        else:
            node.get_logger().error('Service call failed')
            return "ERROR:Service call failed"
    except Exception as e:
        node.get_logger().error(f'Unexpected error: {str(e)}')
        return f"ERROR:{str(e)}"
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    print(main())
EOF
    )
    
    # 返回结果
    echo "$result"
}

load_params_to_map() {
    local params_output="$1"
    
    param_map=()
    
    while IFS= read -r line; do
        if [[ -z "$line" || "$line" == ERROR* ]]; then
            continue
        fi
        
        # 分割参数名和值
        IFS=',' read -r name value <<< "$line"
        
        param_map["$name"]="$value"
    done <<< "$params_output"
}

get_param_value() {
    local key="$1"
    if [ ${#param_map[@]} -eq 0 ]; then
        # 如果没有加载参数，则加载
        if ! load_params_to_map "$PARAMS" ; then
            log "ERROR" "加载参数失败"
            return 1
        fi
    fi
    
    if [ -v param_map["$key"] ]; then
        echo "${param_map[$key]}"
        return 0
    else
        log "ERROR" "参数未找到: $key"
        return 1
    fi
}

print_param_map() {
    log "INFO" "参数映射内容:"
    for key in "${!param_map[@]}"; do
        log "INFO" "  $key = ${param_map[$key]}"
    done
}

get_ros2_param() {
    get_param_value "$1"
}

dump_param_map() {
    if [ ${#param_map[@]} -eq 0 ]; then
        log "WARN" "参数映射为空"
        return 1
    fi

    log "INFO" "===== 参数映射内容 ====="
    for key in "${!param_map[@]}"; do
        log "DEBUG" "$key = ${param_map[$key]}"
    done
    log "INFO" "===== 共 ${#param_map[@]} 个参数 ====="
    return 0
}

write_params_to_file() {
    local output_file="${1:-/tmp/agv_params.txt}"
    
    # 检查文件是否已存在
    if [ -f "$output_file" ]; then
        log "WARN" "文件已存在，跳过写入: $output_file"
        return 1
    fi
    
    if [ ${#param_map[@]} -eq 0 ]; then
        log "WARN" "参数映射为空，无法写入文件"
        return 1
    fi
    
    > "$output_file"
    
    for key in "${!param_map[@]}"; do
        echo "$key=${param_map[$key]}" >> "$output_file"
    done
    
    log "INFO" "参数映射已写入文件: $output_file"
    return 0
}

read_params_from_file() {
    local input_file="${1:-/tmp/agv_params.txt}"
    
    # 检查文件是否存在
    if [ ! -f "$input_file" ]; then
        log "WARN" "参数文件不存在: $input_file"
        return 1
    fi
    
    # 检查文件是否可读
    if [ ! -r "$input_file" ]; then
        log "WARN" "参数文件不可读: $input_file"
        return 1
    fi
    
    # 清空现有的参数映射（可选，根据需求决定是否保留注释）
    # 如果需要保留已有参数，只更新文件中存在的键，请移除下一行
    # param_map=()
    
    local line_count=0
    local valid_count=0
    
    # 逐行读取文件
    while IFS= read -r line || [ -n "$line" ]; do
        line_count=$((line_count + 1))
        
        # 跳过空行和注释行（以#开头的行）
        if [[ -z "$line" || "$line" =~ ^[[:space:]]*# ]]; then
            continue
        fi
        
        # 提取键值对（支持值中包含等号）
        local key="${line%%=*}"
        local value="${line#*=}"
        
        # 检查键是否有效（非空且不包含空格）
        if [[ -z "$key" || "$key" =~ [[:space:]] ]]; then
            log "WARN" "行 $line_count: 无效的键名 '$key'，跳过"
            continue
        fi
        
        # 更新参数映射
        param_map["$key"]="$value"
        valid_count=$((valid_count + 1))
        
    done < "$input_file"
    
    if [ $valid_count -eq 0 ]; then
        log "WARN" "文件中未找到有效的参数键值对: $input_file"
        return 1
    fi
    
    log "INFO" "从文件成功读取 $valid_count 个参数: $input_file"
    return 0
}

monitor_params_server() {
    # 确保参数映射已加载
    if [ ${#param_map[@]} -eq 0 ]; then
        if ! load_params_to_map "$PARAMS"; then
            log "ERROR" "Failed to load parameters" 
            return 1
        fi
    fi

    # 获取参数值
    local init_state
    if ! init_state=$(get_param_value "shared_parameters.error_code.value"); then
        log "ERROR" "Failed to get parameter value" 
        return 2
    fi

    # 输出参数值
    # echo "$init_state"
    return 0 
}

set_log_level() {
    LOG_LEVEL=$1
    log "INFO" "修改日志等级:$LOG_LEVEL"
}

get_log_level() {
    echo "Log level $LOG_LEVEL"
}

log() {
    local level="$1"
    local message="$2"
    local timestamp=$(date +"%Y-%m-%d %H:%M:%S")

    local -A level_weights=(
        ["DEBUG"]=0
        ["INFO"]=1
        ["WARN"]=2
        ["ERROR"]=3
    )
    
    local current_weight=${level_weights[$LOG_LEVEL]}
    local msg_weight=${level_weights[$level]}
    
    if [[ -z "$current_weight" || -z "$msg_weight" ]]; then
        echo "[$timestamp] [ERROR] 无效的日志级别: $level" >> "$LOG_FILE"
        return 1
    fi
    
    if (( msg_weight >= current_weight )); then
        echo "[$timestamp] [$level] $message" >> "$LOG_FILE"
    fi
}

launch_node() {
    local node_name="$1"
    shift
    log "INFO" "启动节点: $node_name"
    
    # 启动节点并重定向输出
    "$@" > /dev/null 2>&1 &
    local pid=$!
    
    # 将PID写入文件
    echo "$pid" >> /tmp/process.pid
    
    # 记录进程信息
    PROC_ENTRIES+=("${node_name}:$pid")
    log "INFO" "节点 $node_name 已启动 (PID: $pid)"
    
    sleep 0.3  # 等待节点初始化
    return 0
}

cleanup_processes() {
    # 检查PID文件是否存在
    if [ ! -f /tmp/process.pid ]; then
        log "WARN" "进程PID文件不存在: /tmp/process.pid"
        return 0
    fi
    
    log "INFO" "开始清理进程..."
    
    # 读取PID文件并终止所有进程及其子进程
    while read -r pid; do
        # 检查PID是否有效
        if [ -z "$pid" ] || ! [[ "$pid" =~ ^[0-9]+$ ]]; then
            log "WARN" "无效的PID: '$pid'"
            continue
        fi
        
        # 检查进程是否存在
        if ps -p "$pid" >/dev/null 2>&1; then
            # 获取进程名称（使用更兼容的方式）
            process_name=$(ps -o comm= -p "$pid" 2>/dev/null || echo "unknown process")
            
            log "INFO" "终止进程树: $process_name (PID: $pid)"
            
            # 终止所有子进程
            pkill -P "$pid" 2>/dev/null
            
            # 终止父进程
            kill -9 "$pid" 2>/dev/null
            
            # 确保进程已被终止
            if ps -p "$pid" >/dev/null 2>&1; then
                log "WARN" "进程 $pid 未能终止，尝试强制终止"
                kill -9 "$pid" 2>/dev/null
            fi
        else
            log "INFO" "进程已不存在 (PID: $pid)"
        fi
    done < /tmp/process.pid
    
    # 清理PID文件
    rm -f /tmp/process.pid
    log "INFO" "清理完成"
    return 0
}